#include "common.h"
#include "socket.h"
#include "drv_fpga.h"
// 执行主要的业务流程
//等待线程唤醒, 需要配合锁和唤醒机制相互配合使用
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
pthread_cond_t has_producer = PTHREAD_COND_INITIALIZER;
extern uint8 net_buf[VALN_BUF_LEN];
extern FanShapedSector fShapedSector[SECTOR_NUM];  //扇区角度范围
extern uint8 IS_CALIB;
extern uint8 IS_COEFF;
int fix_freq[ABP_FREQ_NUM];
int scan_fshaped_type = 0;                  //扇形扫描相关参数, 扫描一次, 还是周期扫面

volatile int scan_flag = END___SCAN;        //没有扫描
int scan_cros = 0;                          //需要交叉切换参数    
int scan_type = 0;                          //扫描类型
int scan_freq_index = 0;                    //当前频率
// int cycleTras = 208;                     //1.08 ms
int cycleTras = 108;                        //1.08 ms
double intervaldeg = 0;                     //相邻两个波位之间的间隔度数
double intervaldeg1 = 0;                    //相邻两个波位之间的间隔度数
double deg_true = 0;                        //指定角度扫描

int InitAllZero();
int getNearFreq(int freq);
int file_Download(uint8 * rbuf);
int query_amplit(Para_Model *pmodel);
void get_index_head(Para_Model *pmodel);
int file_Download(uint8 * rbuf);

// 关闭所有通道
int close_AllChannel() {
    InitAllZero();
    LOG_INFO("all channel are closed");
}

int get_diff_deg(Para_Model *paraModel) {
    int num = (int)(paraModel->cur_deg / DEG_INTER_HALF);                               //整除
    double remainder = paraModel->cur_deg - (num * DEG_INTER_HALF);                     //余数
    int temp = (int)(remainder * 100000);                                               //临界值
    if(num%2 == 0) {
        paraModel->diff_deg = remainder; 
        paraModel->start_channel = num/2 + 1;
    } else {
        paraModel->start_channel = (int)(num/2) + 2;
        paraModel->diff_deg = remainder - DEG_INTER_HALF; 
    }
    paraModel->start_channel = paraModel->start_channel == 129 ? 1: paraModel->start_channel;
    // LOG_DEBUG("cur_deg: %.6lf, start_channel: %d, diff_deg: %.6lf, num: %d ", paraModel->cur_deg, paraModel->start_channel, paraModel->diff_deg, num);
    return KT_OK;
}

int openChannel(Para_Model *paraModel) {
    // 根据起始角度计算出要打开的起始通道和相位差
    int check = (int)(paraModel->cur_deg * DEG_FOR_COMPAR);
    int temp = check - DEG_MAX_360*DEG_FOR_COMPAR;
    // LOG_DEBUG("%d %d", check, temp);
    if(temp >= 0 || check < 0) {
        LOG_ERROR("cur_deg rang is error; cur_deg: %.6lf", paraModel->cur_deg);
        return KT_ERROR;
    }
    get_diff_deg(paraModel);
    get_index_head(paraModel);
    return query_amplit(paraModel);
}
extern uint8 *calibErrordata;
int scanning() {
    Para_Model pmod;
    pmod.itrdeg1 = intervaldeg;
    pmod.itrdeg2 = intervaldeg1;
    pmod.scan_type = scan_type;
    pmod.cur_deg = deg_true;
    pmod.index = 0;
    pmod.scan_freq = fix_freq[scan_freq_index];

    /** 按照u8字节去计算， 防止搅浑 */
    uint8 * cptr = (uint8 *)calibErrordata;
    int offset = 128 * scan_freq_index * 2;     //相对于一个缓冲区内频点的偏移量

    pmod.calib_tx_damp  = cptr + CalibErrorOneLEN*0 + offset;
    pmod.calib_tx_phase = cptr + CalibErrorOneLEN*1 + offset;
    pmod.calib_rx_damp  = cptr + CalibErrorOneLEN*2 + offset;
    pmod.calib_rx_phase = cptr + CalibErrorOneLEN*3 + offset;

    // printHex1(pmod.calib_tx_damp, 128*2, 128*2, "calib_tx_damp");
    // printHex1(pmod.calib_tx_phase, 128*2, 128*2, "calib_tx_phase");
    // printHex1(pmod.calib_rx_damp, 128*2, 128*2, "calib_rx_damp");
    // printHex1(pmod.calib_rx_phase, 128*2, 128*2, "calib_rx_phase");

    LOG_DEBUG("cptr %p, calibErrordata %p", cptr, calibErrordata);
    LOG_DEBUG("calib_tx_damp: %p,calib_tx_phase: %p, calib_rx_damp: %p,calib_rx_phase: %p, offset: %d", \
            pmod.calib_tx_damp, pmod.calib_tx_phase, pmod.calib_rx_damp, pmod.calib_rx_phase, offset);

    double temp = 0.0;

    if(scan_type == Test_Verify) {                          //校验, 验证指定角度打开的通道是否正确
        openChannel(&pmod);
        goto GO_BACK;
    }

    if(scan_type == Scan_Circle) {
        while(1) {

            if(scan_flag == END___SCAN) {
                goto GO_BACK;
            }

            openChannel(&pmod);

            // pmod.cur_deg = scan_cros ? pmod.cur_deg + (pmod.index % 2 == 0 ? pmod.itrdeg1 : pmod.itrdeg2) : pmod.cur_deg + pmod.itrdeg1;
            pmod.cur_deg = pmod.cur_deg + (pmod.index % 2 == 0 ? pmod.itrdeg1 : pmod.itrdeg2);

            temp = pmod.cur_deg - DEG_MAX_360;

            // 这里如果这样子写的话, 0° 不是波位相, 波形在扫描一圈之后会出现断续的情况, 后续看客户对误差的要求如何
            if((int)(temp*1000000) >= 0) {
                pmod.cur_deg = temp;
                pmod.index = 0;
                Sector1_pulse;
            }
            pmod.index++;
        }
    }

    if(scan_type == Scan_Fanshaped) {
        int cnt = 0;
        FanShapedSector fsdtor;
        while(1) {

            if(scan_flag == END___SCAN) {
                goto GO_BACK;
            }
            
            pmod.index = 0;
            for(cnt = 0; cnt < SECTOR_NUM; cnt++) {         //三个扇区轮番扫描
                fsdtor = fShapedSector[cnt]; 
                if(fsdtor.valid == 0x00) {
                    continue;
                }
                Sector_pulse(cnt);
                pmod.cur_deg = fsdtor.start_deg * DEG_UINT_NET;
                while(1) {

                    if(scan_flag == END___SCAN) {
                        goto GO_BACK;
                    }

                    openChannel(&pmod);

                    pmod.cur_deg = scan_cros ? pmod.cur_deg + (pmod.index % 2 == 0 ? pmod.itrdeg1 : pmod.itrdeg2) : pmod.cur_deg + pmod.itrdeg1;
                    
                    temp = pmod.cur_deg - fsdtor.end_deg * DEG_UINT_NET;
                    if((int)(temp*1000000) >= 0) {
                        break;
                    }
                }

            }
            if(scan_fshaped_type == FanShaped_One) {
                goto GO_BACK;
            }
        }
    }

GO_BACK:
    scan_flag = END___SCAN;         //结束扫描
    LOG_INFO("module scan end ....");
}

void * thread_scanning() {
    prctl(PR_SET_NAME, "SCAN");
    pthread_detach(pthread_self());

    if(scan_flag == END___SCAN) {
        LOG_DEBUG("end scanning circle");
    }
    if(scanning() == KT_ERROR) {
        LOG_ERROR("scanning circle error");
    }
}

int doScan() {
    pthread_t scanning_thread_id;
    int res = pthread_create(&scanning_thread_id, NULL, thread_scanning, NULL);
    if(!res) {
        return KT_OK;
    }
    LOG_ERROR("create thread_scanning failure, errCode: %d", res);
    return KT_ERROR;
}

int initScan(uint8 *rbuf) {                 //初始化共有的参数
    int res = KT_OK;
    int cur_speed = rbuf[scanSpeed];        //当前转速
    scan_freq_index = getNearFreq(rbuf[scanFreq]*10 + 9000);
    scan_cros = 0;
    intervaldeg1 = 0;
    LOG_INFO("The current frequency is %dMHz", fix_freq[scan_freq_index]);
    if(cycleTras == 108) {          //同步信号周期 108
        LOG_INFO("Synchronization signal period: 1.08ms");
        if(cur_speed == Speed_03_48) {
            intervaldeg = 0.3125;
            LOG_INFO("Current speed %dr/min", 48);
        } else if(cur_speed == Speed_02_24) {
            intervaldeg = 0.15625;
            LOG_INFO("Current speed %dr/min", 24);
        } else if(cur_speed == Speed_01_10) {
            intervaldeg = 0.0654;
            LOG_INFO("Current speed %dr/min", 10);
        } else {
            LOG_ERROR("The current rate scan mode is not supported");
            return KT_ERROR;
        }
        LOG_INFO("degrees between wave positions of number: %lf ", intervaldeg);
    } else if(cycleTras == 208) {   //同步信号周期 208
        LOG_INFO("Synchronization signal period: 2.08ms");
        if(cur_speed == Speed_03_48) {
            intervaldeg = 0.703125;
            intervaldeg1 = 0.5625;
            scan_cros = 1;
            LOG_INFO("Current speed %dr/min", 48);
        } else if(cur_speed == Speed_02_24) {
            intervaldeg = 0.3125;
            LOG_INFO("Current speed %dr/min", 24);
        } else if(cur_speed == Speed_01_10) {
            intervaldeg = 0.12784;
            LOG_INFO("Current speed %dr/min", 10);
        } else {
            LOG_ERROR("The current rate scan mode is not supported");
            return KT_ERROR;
        }
        if(scan_cros) {
            LOG_INFO("degrees wave deg1: %lf°, deg2: %lf°", intervaldeg, intervaldeg1);
        } else {
            LOG_INFO("degrees wave deg: %lf° ", intervaldeg);
        }
    } else {
        LOG_ERROR("Parameter error, please check whether the value is correct");
        return KT_ERROR;
    }
    res = doScan();
    return res;
}
void Send_Ctr(int val);
void module_scan(uint8 *rbuf) {
    int res = KT_OK;
    double uint_deg = DEG_UINT_NET;

    //check scan status
    if(rbuf[scanSpeed] == Speed_STOP) {
        if(scan_flag == END___SCAN) {
            LOG_INFO("Module scan is already closed");
            return;
        }
        scan_flag = END___SCAN;
        scan_type = Scan_NoModule;
        usleep(1000*10);
        close_AllChannel();
        LOG_INFO("module scan end .... ");
        return;
    }

    if(scan_flag == START_SCAN) {
        LOG_INFO("program has started scanning ...... ");
        return;
    }

    /* 初始化一些参数 */
    IS_CALIB = Status_OPEN;
    IS_COEFF = Status_OPEN;

    if(rbuf[scanType] == Scan_Circle) {             //圆形扫描
        scan_type = Scan_Circle;
        LOG_INFO("scan type: Scan_Circle");
    } else if(rbuf[scanType] == Scan_Fanshaped) {   //扇形扫描
        // init fanshaped scan type
        scan_fshaped_type = rbuf[FanShapedType];
        if(scan_fshaped_type == FanShaped_Close || scan_fshaped_type == FanShaped_Open) {
            scan_type = Scan_NoModule;
            Send_Ctr(scan_fshaped_type == FanShaped_Close ? 0 : 1); //发射控制关闭
        } else {
            //init other para
            scan_type = Scan_Fanshaped;
            LOG_INFO("scanning type: Scan_Fanshaped");
            LOG_INFO("scanning period: %s", scan_fshaped_type == 0x00 ? "Scan Period" : "Scan Only One");
            // init sector 
            fShapedSector[0].valid = rbuf[FanShapedSector1_V];
            fShapedSector[1].valid = rbuf[FanShapedSector2_V];
            fShapedSector[2].valid = rbuf[FanShapedSector3_V];
            if(rbuf[FanShapedSector1_V] == FS_SECTOR_VALID_YES) {
                fShapedSector[0].start_deg = ((rbuf[FanShapedSector1_S_L] & 0x00FF) << 8) | rbuf[FanShapedSector1_S_H];
                fShapedSector[0].end_deg = ((rbuf[FanShapedSector1_E_L] & 0x00FF) << 8) | rbuf[FanShapedSector1_E_H];
                LOG_INFO("sector1 start deg: %.2lf°, end deg: %.2lf°", (double)(fShapedSector[0].start_deg*uint_deg), (double)(fShapedSector[0].end_deg*uint_deg));
            }
            if(rbuf[FanShapedSector2_V] == FS_SECTOR_VALID_YES) {
                fShapedSector[1].start_deg = ((rbuf[FanShapedSector2_S_L] & 0x00FF) << 8) | rbuf[FanShapedSector2_S_H];
                fShapedSector[1].end_deg = ((rbuf[FanShapedSector2_E_L] & 0x00FF) << 8) | rbuf[FanShapedSector2_E_H];
                LOG_INFO("sector2 start deg: %.2lf°, end deg: %.2lf°", (double)(fShapedSector[1].start_deg*uint_deg), (double)(fShapedSector[1].end_deg*uint_deg));
            }
            if(rbuf[FanShapedSector3_V] == FS_SECTOR_VALID_YES) {
                fShapedSector[2].start_deg = ((rbuf[FanShapedSector3_S_L] & 0x00FF) << 8) | rbuf[FanShapedSector3_S_H];
                fShapedSector[2].end_deg = ((rbuf[FanShapedSector3_E_L] & 0x00FF) << 8) | rbuf[FanShapedSector3_E_H];
                LOG_INFO("sector3 start deg: %.2lf°, end deg: %.2lf°", (double)(fShapedSector[2].start_deg*uint_deg), (double)(fShapedSector[2].end_deg*uint_deg));
            }
        }
        
    } else if(rbuf[scanType] == Test_Verify) {  //打开指定角度, 扫描 
        scan_type = Test_Verify;
        // intervaldeg =   0.0654;  // 默认10转
#if MODULE_TEST
        //2.8125
        // deg_true = 43.6;
        deg_true = 120;
        // deg_true = 1.41;
        // deg_true = 1.40625;
        // deg_true = 1.41625;
        // deg_true = 0;
        deg_true = deg_true*100;
        IS_CALIB = rbuf[4];
        IS_COEFF = rbuf[5];

        IS_CALIB = 1;
        IS_COEFF = 1;
#else
        deg_true = ((rbuf[7] & 0x00FF) << 8) | (rbuf[6] & 0xFF);
        IS_CALIB = rbuf[4];
        IS_COEFF = rbuf[5];
#endif  
        if(IS_CALIB) {
            LOG_INFO("load IS_CALIB data");
        }
        if(IS_COEFF) {
            LOG_INFO("load IS_COEFF data");
        }
        deg_true = deg_true*DEG_UINT_NET;
        LOG_INFO("scan type: true deg scan, deg: %.2lf°", deg_true);
        scan_freq_index = getNearFreq(rbuf[scanFreq]*10 + 9000);
        LOG_INFO("The current frequency is %dMHz, scan_index: %d", fix_freq[scan_freq_index], scan_freq_index);
        res = doScan();       //直接进行扫描模式
        goto GO_BACK;
    } else {
        LOG_ERROR("The current scanning mode is not supported");
        scan_type = Scan_NoModule;
    }

    if(scan_type != Scan_NoModule) {
        res = initScan(rbuf);
    }

GO_BACK:
    if(res == KT_OK) {
        scan_flag = START_SCAN;
    } else {
        scan_flag = END___SCAN;
    }
}
void ConTrolFanFun(uint8 *cbuf);
void sendStatusTR(int type);
void Power_TxCtr(uint8 *cbuf);
void pz_del_netdata(uint8 *rbuf);
void del_cmd(uint8 *rbuf) {
#if MODULE_TEST
    // 圆扫
    // uint8 cbuf[] = { 0xbb, 0x01, 0x01, 0x29, 0x00, 0x00, 0xfe, 0xee }; // 10转, 9410MHz
    // uint8 cbuf[] = { 0xbb, 0x01, 0x02, 0x1f, 0x00, 0x00, 0xfe, 0xee }; // 24转, 9310MHz
    // 停止指令
    // uint8 cbuf[] = { 0xbb, 0x01, 0x00, 0x1f, 0x00, 0x00, 0xfe, 0xee }; // 24转, 9310MHz
    // 扫一次,48r, 扇区1: 0~100°,扇区2: 120~200, 9510Mhz
    // uint8 cbuf[] = { 0xbb, 0x02, 0x03, 0x33, 0x01, 0x01, 0x00, 0x00, 0xe8, 0x03, 0x01, 0xb0, 0x04, 0xd0, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd }; // 24转, 9310MHz
    // 扫一次,48r, 扇区1: 0~100°,扇区2: 120~200,扇区3:240~359; 9510Mhz
    // uint8 cbuf[] = {0xbb,0x02,0x03,0x33,0x01,0x01,0x00,0x00,0xe8,0x03,0x01,0xb0,0x04,0xd0,0x07,0x01,0x60,0x09,0x06,0x0e,0xfd,0xee}; 
    // 同上, 加小数点, 扇区3:240~348.45
    // uint8 cbuf[] = {0xbb,0x02,0x03,0x33,0x01,0x01,0x00,0x00,0xe8,0x03,0x01,0xb0,0x04,0xd0,0x07,0x01,0x60,0x09,0x9c,0x0d,0xfd,0xee};
    // 同上, 周期扫
    uint8 cbuf[] = {0xbb,0x02,0x03,0x33,0x00,0x01,0x00,0x00,0xe8,0x03,0x01,0xb0,0x04,0xd0,0x07,0x01,0x60,0x09,0x9c,0x0d,0xfd,0xee}; 
    // 测向启停止
    // uint8 cbuf[] = {0xbb,0x03,0x01,0x1f,0x01,0xfc,0xee};
    // 指向角度 7°
    // uint8 cbuf[] = {0xbb,0x04,0x01,0x1f,0x00,0x01,0x46,0x00,0xfb,0xee};
    // 指向角度 287.45°
    // uint8 cbuf[] = {0x9b,0x04,0x01,0x33,0x01,0x01,0x46,0x00,0xfb,0xee};
    // uint8 cbuf[] = {0xbb,0x04,0x01,0x1f,0x00,0x01,0x46,0x00,0xfb,0xee};
    // uint8 cbuf[] = {0xbb,0x04,0x01,0x1f,0x00,0x00,0x46,0x00,0xfb,0xee};
    // 文件下载
    // uint8 cbuf[] = {0xbb, 0x10, 0xE8, 0x03, 0xE8, 0x03, 0x01, 0x00};

    memset(rbuf, 0x0, VALN_BUF_LEN);
    memcpy(rbuf, cbuf, sizeof(cbuf));
#endif  // MODULE_TEST
    // printHex(rbuf, 40, "recv network data:");
    uint8 cmdType = rbuf[scanType];                //指令类型
    if(rbuf[head] == 0xBB) {
        if(cmdType == Scan_Circle || cmdType == Scan_Fanshaped || cmdType == Test_Verify) {
            LOG_INFO("module is scan ");
            module_scan(rbuf);
        } else if(cmdType == Test_Startup) {
            LOG_INFO("module is Test_Startup ");
            
        } else if(cmdType == Query_StatusTR) {
            LOG_INFO("module is Query_StatusTR ");
            sendStatusTR(1);
        } else if(cmdType == Gain_CtrRxModule) {
            LOG_INFO("module is Gain_CtrRxModule ");

        } else if(cmdType == File_Download) {
            LOG_INFO("module is file download ");
            file_Download(rbuf);
        } else if(cmdType == Power_TxModule) {
            LOG_INFO("module is Power control ");
            Power_TxCtr(rbuf);
        } else if(cmdType == ConTrolFan) {
            LOG_INFO("Control fan speed, pwm: %d ", rbuf[3]);
            ConTrolFanFun(rbuf);
        } else {
            LOG_WARN("no suport cur module, type module is %d", cmdType);
        }
    }else if(rbuf[head] == 0x9C) {
        LOG_INFO("module is PZ data forward");
        pz_del_netdata(rbuf);
    }else {
        LOG_WARN("network cmd head is not 0xBB or 0x9B,  it is %02x", rbuf[head]);
    }
}

void * delcmd_thread() {
    prctl(PR_SET_NAME, "DCMD");
    while(1) {
        while (1) {
            int res = pthread_cond_wait(&has_producer, &mutex);   //唤醒
            if(res == 0) {
                break;
            }
        }
        LOG_DEBUG("scan thread start ... ");
        del_cmd(net_buf);
    }
}

void usr_app() {
    pthread_t scan_threadId;
    pthread_create(&scan_threadId, NULL, delcmd_thread, (void *)NULL);
}